
from pyb import millis
from math import pi,isnan

#取绝对值函数
def ABS(imax):
    if imax > 0:
        imax=imax
    elif imax < 0:
        imax=-imax

#pid类，包含数据储存与方法
class PID:

    _KP= _KI = _KD = _integrator = _imax =0
    _last_error=_last_derivetive=_last_t=0
    _RC=1/(2 * pi * 20)

    def __init__(self,p,i,d,imax):

        self._KP=float(p)
        self._KI=float(i)
        self._KD=float(d)
        self._imax=ABS(imax)
        self._last_derivetive=float('nan')

    def Git_Pid(self,error,scaler):

        t_now = millis()
        dt=t_now-self._last_t
        out_put=0
        if self._last_t == 0 or dt > 1000:
            dt=0
            self.reset_t()
        self._last_t=t_now
        delat_time = float(dt) / float(1000)
        #计算kp加入out_put
        out_put+=self._KP * error

        #计算ki加入out_put
        if ABS(self._KI > 0) and dt > 0 :
            self._integrator += (error *self._KI)* scaler *\
                                    delat_time
            if self._integrator < -self._imax :
                self._integrator = -self._imax
            if self._integrator > self._imax :
                self._integrator = self._imax
            out_put += self._integrator

        #计算kd加入out_put
        if ABS(self._KD > 0) and dt > 0 :
            if isnan(self._last_derivetive) :
                derivetive = 0
                self._last_derivetive = 0
            else :
                derivetive = (error -self._last_error)/delat_time
            derivetive=self._last_derivetive+\
                        ((delat_time/(self._RC + delat_time))*\
                        (derivetive - self._last_derivetive))
            self._last_error=error
            self._last_derivetive=derivetive
            out_put+=derivetive
        out_put *=scaler
        return out_put

    def reset_i(self):
        self._integrator=0
        self._last_derivetive=float('nan')






